Autonomously and semi-autonomously controlled machines are capable of operating with little or no human input by relying on information received from various machine systems. Control of the machines may be dependent on navigational data provided by various sensors mounted onboard the machines. In order for the machines to operate properly, the information provided by the sensors must be accurate and reliable.
For example, an inertial measurement unit (IMU) is often used to provide pose information of a machine. The pose information may include, for example, one or more of position, attitude, linear velocity, angular velocity, and acceleration. The IMU, however, is not ideal, and may have noise, bias, or drift due to, for example, aging and temperature. The noise, bias, and drift may accumulate, affecting the accuracy of the determined pose. Thus, an independent pose measurement, such as global positioning system (GPS) signals, may be used to update the readings of the IMU. Typically, a Kalman filter may be used to account for bias and scale errors in the IMU by utilizing the GPS data.
U.S. Pat. No. 8,996,311 of Morin, which issued on Mar. 31, 2015 (the '311 patent), discloses a navigation system using an IMU for navigating a vehicle and a GPS to correct accumulated errors from the IMU. To produce the navigation information, the system performs two main processes, the mechanization of the raw IMU data into a trajectory (i.e., a time series of position, velocity, and attitude) and the correction of that trajectory with updates estimated by an extended Kalman filter. The mechanization occurs at the rate of the IMU data, usually higher than 100 Hz. The Kalman filter runs at a lower rate, for example, at 1 Hz, such that errors in the trajectory accumulate to become clearly observable when compared to the GPS information. The lower rate tends to keep the updates sufficiently separated in time to eliminate (or at least mitigate) time correlated errors on the update measurements.
Although the system of the '311 patent may accurately navigate the vehicle, the error accumulation creates noticeable discontinuities in the trajectory of the IMU data. In many machine control applications, having a smooth trajectory is desired. For example, during the control of a steering wheel of a truck or a blade on a tractor, a sudden change of velocity or orientation not only may cause discomfort to the machine operator, but also may lead to dangerous maneuvers.
The disclosed system is directed to overcoming one or more of the problems set forth above and/or other problems of the prior art.